
#include <AFMotor.h>
#include <SoftwareSerial.h>

// Set up a new SoftwareSerial object with RX in digital pin 10 and TX in digital pin 11
SoftwareSerial mySerial(10, 9);
AF_DCMotor motor1(1);
AF_DCMotor motor2(2);

AF_DCMotor motor[2]={motor1,motor2};
#define MOTORNUM  2
//speed 小于120时，启动不了
void forward_drive(int speed)
{
 
 for(char i=0;i<MOTORNUM;i++)
  {
     motor[i].run(FORWARD);
     motor[i].setSpeed(speed); 
   // motor[i].run(RELEASE);

  }
}
void left_drive(int speed1,int speed2)
{
    motor[0].run(FORWARD);
    motor[0].setSpeed(speed1); 
  // motor[i].run(RELEASE);
    motor[1].run(FORWARD); 
    motor[1].setSpeed(speed2); 
  


}
void right_drive(int speed1,int speed2)
{
    motor[0].run(FORWARD);
    motor[0].setSpeed(speed2); 
  // motor[i].run(RELEASE);
    motor[1].run(FORWARD); 
    motor[1].setSpeed(speed1); 
 


}
void backward_drive(int speed)
{
for(char i=0;i<MOTORNUM;i++)
  {
     motor[i].run(BACKWARD);
      motor[i].setSpeed(speed); 
   // motor[i].run(RELEASE);

  }

}
void rotate(int rot)
{
   char dir=0;
   char dir_1=0;
  if(rot>0)
  {
    dir=BACKWARD;
    dir_1=FORWARD;


  }
  else
  {
    dir=FORWARD;
    dir_1=BACKWARD;

  }
 
  for(char i=0;i<MOTORNUM;i++)
  {
      if(i==0||i==2)
      {
        motor[i].run(dir);
        motor[i].setSpeed(rot); 
      }
      if(i==1||i==3)
      {
        motor[i].run(dir_1);
        motor[i].setSpeed(rot); 
      }
   // motor[i].run(RELEASE);
  }

  
}
void stop()
{
  for(char i=0;i<MOTORNUM;i++)
  {
    motor[i].setSpeed(0); 
    motor[i].run(RELEASE);

  }

}
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("Motor test!");
  mySerial.begin(9600);
  // turn on motor
 for(char i=0;i<MOTORNUM;i++)
  {
    motor[i].setSpeed(200); 
    motor[i].run(RELEASE);

  }

 
 
}

void loop() {
  // put your main code here, to run repeatedly:
//前进
  if(mySerial.available()>0)
    {
      char c = mySerial.read();
      Serial.println(c);
      if(c=='1')
      {
        forward_drive(120); 

      }
     else if(c=='0')
      {

        stop();

      }
     else if(c=='2')
      {
        backward_drive(120);

      }
      else if(c=='3')
      {
        rotate(120);
      }
      else if(c=='4')
      {
        rotate(-120);

      }
      else if(c=='5')
      {
        right_drive(0,120);

      }
       else if(c=='6')
      {
        right_drive(120,20);

      }
      else
      {

        stop();
      }
    }

   

  //step2. back 1m

}
